import cv2
import sys
import os
import time
import queue
import numpy as np
import multiprocessing
from process_task.multiprocess_task import *

# sudo systemctl enable lightdm
# sudo systemctl start lightdm

# sudo systemctl disable lightdm 
# sudo systemctl stop lightdm


RUNNING_FLAG = multiprocessing.Event() # 所有任务运行标志
RUNNING_FLAG.set()  # 设置为True，表示所有任务正在运行

ENABLE_FRAME_1 = multiprocessing.Value('b', False)      # 主摄像头显示 :  True开启  False关闭
ENABLE_HDMI_DISPLAY = multiprocessing.Value('b', False)      # HDMI显示和触摸 :  True开启  False关闭
ENABLE_YOLOV8_DETECT = multiprocessing.Value('b', False)    # yolov8图像检测:  True: 启用, False: 禁用
ENABLE_FIND_COUNTS = multiprocessing.Value('b',  False)      # 轮廓检测:  True: 启用, False: 禁用
ENABLE_GEMINI_336L  = multiprocessing.Value('b', False)    # 双目摄像头显示 :  True开启  False关闭

ENABLE_TOUCH_SCREEN = multiprocessing.Value('b', True)    # 触摸屏控制:  True: 启用, False: 禁用

# 串口控制:  True: 启用, False: 禁用
ENABLE_UART1 = multiprocessing.Value('b', False)
ENABLE_UART2 = multiprocessing.Value('b', False)
ENABLE_UART3 = multiprocessing.Value('b', False)
ENABLE_UART6 = multiprocessing.Value('b', False)

COLOR_MODE = multiprocessing.Value('i', 3)

# 串口控制: 队列
serial1_write = multiprocessing.Queue()
serial1_read  = multiprocessing.Queue()

serial2_write = multiprocessing.Queue()
serial2_read  = multiprocessing.Queue()

serial3_write = multiprocessing.Queue()
serial3_read  = multiprocessing.Queue()

serial6_write = multiprocessing.Queue()
serial6_read = multiprocessing.Queue()

# # 根据开关决定是否创建队列
# ENABLE_FRAME = False  # 如果不需要，就设为 False
# frame1_queue = multiprocessing.Queue(maxsize=5) if ENABLE_FRAME else None

frame1_queue = multiprocessing.Queue(maxsize=5) #  创建一个队列用于从摄像头进程接收帧
hdmi_queue = multiprocessing.Queue(maxsize=5) #   创建一个队列用于HDMI显示进程接收帧
data_result_queue = multiprocessing.Queue(maxsize=10) # 用于发送识别结果给下位机
touch_data_queue = multiprocessing.Queue(maxsize=10) # 用于发送触摸数据


CONTROL_FLAGS_MAP = {
    "frame1": ENABLE_FRAME_1,
    "gemini336l": ENABLE_GEMINI_336L,
    "hdmi_display": ENABLE_HDMI_DISPLAY,
    "yolov8_detect": ENABLE_YOLOV8_DETECT,
    "find_counts": ENABLE_FIND_COUNTS,                                                                                              
    "touch_screen": ENABLE_TOUCH_SCREEN,                                                                                                                                                        
    "uart1": ENABLE_UART1,
    "uart2": ENABLE_UART2,
    "uart3": ENABLE_UART3,
    "uart6": ENABLE_UART6,
    "color_mode": COLOR_MODE,# red:1 blue:2 green:3 yellow:4
}

# 串口配置列表 "/dev/ttyS1" "/dev/ttyS2" "/dev/ttyS3" "/dev/ttyS6"
ports = [
    (ENABLE_UART1, "/dev/ttyS1", 230400, serial1_write, serial1_read),
    (ENABLE_UART2, "/dev/ttyS2", 230400, serial2_write, serial2_read),
    (ENABLE_UART3, "/dev/ttyS3", 230400, serial3_write, serial3_read),
    (ENABLE_UART6, "/dev/ttyS6", 230400, serial6_write, serial6_read),
]

# 任务配置
TASK_CONFIG = {
    "start":  { k: True  for k in CONTROL_FLAGS_MAP.keys() },
    "stop":   { **{ k: False for k in CONTROL_FLAGS_MAP.keys() },}, 
    "task1":  { **{ k: True for k in CONTROL_FLAGS_MAP.keys()  }, "gemini336l": False,"color_mode":4}, #检测黄色圆环
    "task2":  { **{ k: True for k in CONTROL_FLAGS_MAP.keys()  }, "gemini336l": False,"color_mode":2},#检测蓝色圆环
    "task3":  { **{ k: True for k in CONTROL_FLAGS_MAP.keys()  }, "gemini336l": False,"color_mode":3,}, #检测绿色圆环
    "task4":  { **{ k: True for k in CONTROL_FLAGS_MAP.keys()  }, "gemini336l": False,"color_mode":1}, #检测绿色圆环
    "task5":  { **{ k: False for k in CONTROL_FLAGS_MAP.keys() }, "gemini336l": False,"color_mode":1},  #检测黄色和蓝色圆环
    "task6":  { **{ k: False for k in CONTROL_FLAGS_MAP.keys() },},
}

def restart_program():
    python = sys.executable      # Python 解释器路径
    os.execl(python, python, *sys.argv)  # 替换当前进程，重新执行


# --- 主进程逻辑 ---
def main_process():

    print("[主进程] 应用程序正在启动...")
    processes = []

    # 准备进程创建所需的信息 
    video1_args = (RUNNING_FLAG, ENABLE_FRAME_1,frame1_queue,'cap_main',{'width': 640, 'height': 480, 'fps': 30})
    serial_args = (RUNNING_FLAG, ports)
    image_proc_args = (RUNNING_FLAG,ENABLE_FRAME_1,ENABLE_GEMINI_336L,COLOR_MODE,ENABLE_HDMI_DISPLAY, ENABLE_YOLOV8_DETECT, ENABLE_FIND_COUNTS,frame1_queue,hdmi_queue, data_result_queue,2)
    hdmi_display_args = (RUNNING_FLAG, ENABLE_HDMI_DISPLAY, hdmi_queue)
    touch_args = (RUNNING_FLAG, ENABLE_TOUCH_SCREEN, touch_data_queue)

    # 1. 发现摄像头设备
    p_video1_frame = multiprocessing.Process(target=camera_reader_process,
                                            args=video1_args,
                                            daemon=True)
    # 存储进程信息，
    processes.append({"name": "video1_frame", "process": p_video1_frame, "target": camera_reader_process, "args": video1_args})
    p_video1_frame.start()

    # 2. 启动串口进程
    p_serial = multiprocessing.Process(target=serial_process_task,
                                       args=serial_args,
                                       daemon=True)
    # 存储进程信息
    processes.append({"name": "Serial", "process": p_serial, "target": serial_process_task, "args": serial_args})
    p_serial.start()

    # 3. 启动图像处理进程
    p_image_proc = multiprocessing.Process(target=image_processing_process_task,
                                        args=image_proc_args,
                                        daemon=True)
    # 存储进程信息
    processes.append({"name": "ImageProcessing", "process": p_image_proc, "target": image_processing_process_task, "args": image_proc_args})
    p_image_proc.start()

    # 4. 启动HDMI显示进程
    p_hdmi_display = multiprocessing.Process(target=hdmi_process_task,
                                            args=hdmi_display_args,
                                            daemon=True)
    # 存储进程信息
    processes.append({"name": "HDMI_Display", "process": p_hdmi_display, "target": hdmi_process_task, "args": hdmi_display_args})
    p_hdmi_display.start()

      # 5. 启动触摸屏控制进程
    p_touch_screen = multiprocessing.Process(target=hdmi_touch_task,
                                            args=touch_args,
                                            daemon=True)
    # 存储进程信息
    processes.append({"name": "TouchScreen", "process": p_touch_screen, "target": hdmi_touch_task, "args": touch_args})
    p_touch_screen.start()


    # --- 用于定时发送串口数据的变量 ---
    last_data_send_time = time.time()
    last_sent_data = None
    serial_interval = 0.01 # 串口发送间隔 10ms
    last_print_time = 0
    print_interval = 1.0  # 每隔1秒打印一次




    try:
        while RUNNING_FLAG.is_set():

            current_time = time.time()
            something_was_done_this_cycle = False
            try :
                btn_id = touch_data_queue.get(block=False)
            except queue.Empty:
                btn_id = None
            if btn_id is not None:
                print(f"[主进程] 收到触摸事件: {btn_id}")

                if btn_id == 0:
                    for flag_name, desired in TASK_CONFIG["stop"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                elif btn_id == 1:
                    for flag_name, desired in TASK_CONFIG["start"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                elif btn_id == 2:
                    RUNNING_FLAG.clear() 
                    time.sleep(0.1) # 等待子进程关闭
                    restart_program()
                    print(f"[主进程] 收到重启信号，正在重启...")
                elif btn_id == 3:
                    RUNNING_FLAG.clear() # 通知所有子进程停止
                    print("[主进程] 收到退出信号，正在关闭...")
                    break
                elif btn_id == 4:
                    for flag_name, desired in TASK_CONFIG["task1"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                elif btn_id == 5:
                    for flag_name, desired in TASK_CONFIG["task2"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                elif btn_id == 6:
                    for flag_name, desired in TASK_CONFIG["task3"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                elif btn_id == 7:
                    for flag_name, desired in TASK_CONFIG["task4"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                elif btn_id == 8:
                    for flag_name, desired in TASK_CONFIG["task5"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                elif btn_id == 9:
                    for flag_name, desired in TASK_CONFIG["task6"].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                else:
                    print(f"[主进程] 收到未知触摸事件: {btn_id}")
             # --- 1. 尝试从 data_result_queue 获取并发送数据 (受 serial_interval 控制频率) ---
            if current_time - last_data_send_time >= serial_interval:
                data_to_send = None
                try:
                    data_to_send = data_result_queue.get(timeout=0.01) # Small timeout, slightly reduced
                except queue.Empty:
                    pass
                if data_to_send:
                    try:
                        yolov8_x = data_to_send.get("yolov8_x", -1)
                        yolov8_y = data_to_send.get("yolov8_y", -1)
                        yolov8_vx = data_to_send.get("yolov8_vx", -1)
                        yolov8_vx = data_to_send.get("yolov8_vy", -1)
                
                        new_center_x = data_to_send.get("center_green_x",-1)
                        new_center_y = data_to_send.get("center_green_y",-1)
                      
                        def offset(px, py, color_x, color_y):
                            if (px == -1 and py == -1) or (color_x == -1 and color_y == -1):
                                return -1, -1
                            if color_x and color_y:
                                return color_x-px, py - color_y
                            return -1, -1

                        # 要处理的颜色键
                        colors = ["red", "blue", "green", "yellow"]
                        offsets = {}

                        for color in colors:
                            color_x = data_to_send.get(f"{color}_x", 0)
                            color_y = data_to_send.get(f"{color}_y", 0)
                            off_x, off_y = offset(new_center_x, new_center_y, color_x, color_y)
                            offsets[f"{color}_offset_x"] = off_x
                            offsets[f"{color}_offset_y"] = off_y

                        red_offest_x , red_offest_y = offsets["red_offset_x"], offsets["red_offset_y"]
                        blue_offest_x,  blue_offest_y  = offsets["blue_offset_x"], offsets["blue_offset_y"]
                        green_offest_x, green_offest_y = offsets["green_offset_x"], offsets["green_offset_y"]
                        yellow_offest_x,yellow_offest_y= offsets["yellow_offset_x"], offsets["yellow_offset_y"]
                    
                        # # 动态构造数据片段
                        # parts = []
                        # if COLOR_MODE.value == 1 :
                        #     parts.append(f"{red_offest_x},{red_offest_y},{p_vx:.2f},{p_vy:.2f}")
                        # elif COLOR_MODE.value == 2:
                        #     parts.append(f"{blue_offest_x},{blue_offest_y},{p_vx:.2f},{p_vy:.2f}")
                        # elif COLOR_MODE.value == 3:
                        #     parts.append(f"{green_offest_x},{green_offest_y},{p_vx:.2f},{p_vy:.2f}")
                        # elif COLOR_MODE.value == 4:
                        #     parts.append(f"{yellow_offest_x},{yellow_offest_y},{p_vx:.2f},{p_vy:.2f}")
                            
                        # # 动态构造数据片段
                        parts = []
                        if COLOR_MODE.value == 1 :
                            parts.append(f"{red_offest_x},{red_offest_y}")
                        elif COLOR_MODE.value == 2:
                            parts.append(f"{blue_offest_x},{blue_offest_y}")
                        elif COLOR_MODE.value == 3:
                            parts.append(f"{green_offest_x},{green_offest_y}")
                        elif COLOR_MODE.value == 4:
                            parts.append(f"{yellow_offest_x},{yellow_offest_y}")
                    
                    
                        # 最终用逗号拼起来
                        pos_data = ",".join(parts)
                        formatted_data = f"{{{pos_data}}}\n"
                        if formatted_data != last_sent_data:
                            last_sent_data = formatted_data
                             # === 定时打印逻辑 ===
                        current_time = time.time()
                        if current_time - last_print_time >= print_interval:
                            print(f"[主进程] 发送坐标数据: {formatted_data}")
                            last_print_time = current_time
                        # 发送
                        try:
                            serial6_write.put(formatted_data, timeout=0.001)
                        except queue.Full:
                            print("[主进程] serial6_write队列满,跳过发送。")

                        something_was_done_this_cycle = True
                    except Exception as e:
                         print(f"[偏差计算异常] {e}")
                
                last_data_send_time = current_time # 不管是否发送成功，都更新时间，保持频率

            # --- 2. 处理来自串口任务的消息 (高优先级) ---
            try:
                # 非阻塞获取命令
                raw_cmd = serial6_read.get(block=False)
                if isinstance(raw_cmd, bytes):
                    raw_cmd = raw_cmd.decode('utf-8', errors='ignore')
                    raw_cmd = raw_cmd.rstrip('\r\n')
                cmd = raw_cmd.strip().lower()
                print(f"[主进程] 收到串口命令: {cmd}") # 打印收到的命令

                if cmd == "reset":
                    RUNNING_FLAG.clear() 
                    time.sleep(0.1) # 等待子进程关闭
                    restart_program()
                    print(f"[主进程] 收到重启信号，正在重启...")
                
                if cmd in TASK_CONFIG:
                    # 1. 更新 flags
                    print(f"[主进程] 执行任务切换到: {cmd}")
                    for flag_name, desired in TASK_CONFIG[cmd].items():
                        CONTROL_FLAGS_MAP[flag_name].value = desired
                        print(f"[主进程]   - 更新 {flag_name} 状态为 {CONTROL_FLAGS_MAP[flag_name].value}")
                
                else:
                    print(f"[主进程] 收到未知命令: {cmd}")

            except queue.Empty:
                pass # 没有收到串口命令，正常

            except Exception as e:
                 print(f"[主进程] 处理串口命令时发生错误: {e}") # 捕获处理命令时的其他异常

            # 如果本循环没有做任何事情,则短暂休眠
            if not something_was_done_this_cycle:
                time.sleep(0.01) 

    except KeyboardInterrupt:
        print("[主进程] 收到KeyboardInterrupt。正在关闭...")
    except Exception as e:
         print(f"[主进程] 主循环发生未捕获异常: {e}")
    finally:
        print("[主进程] 正在启动关闭序列...")
        RUNNING_FLAG.clear() # 通知所有子进程停止

        # 给子进程一点时间响应停止标志
        time.sleep(0.1) # 短暂等待

        # 遍历并尝试终止所有进程
        for p_info in processes:
            p = p_info['process']
            pname = p_info['name']
            if p.is_alive():
                 print(f"[主进程] 正在发送终止信号给 {pname} 进程...")
                 try:
                    p.terminate()
                 except Exception as e:
                    print(f"[主进程] 发送终止信号给 {pname} 时出错: {e}")


        # 再次遍历，等待所有进程退出
        for p_info in processes:
            p = p_info['process']
            pname = p_info['name']
            if p.is_alive():
                print(f"[主进程] 等待 {pname} 进程退出...")
                try:
                    p.join(timeout=2) # 等待更长时间
                    if p.is_alive():
                        print(f"[主进程] {pname} 进程未正常退出，可能需要手动检查。")
                except Exception as e:
                    print(f"[主进程] 等待 {pname} 退出时出错: {e}")
            else:
                print(f"[主进程] {pname} 进程已退出。")


        # Close queues
        print("[主进程] 正在关闭队列...")
        queues_to_close = [
            serial1_write, serial1_read, serial2_write, serial2_read,
            serial3_write, serial3_read, serial6_write, serial6_read,
            frame1_queue,  hdmi_queue, data_result_queue,touch_data_queue
        ]

        for q_obj in queues_to_close:
            try:
                q_obj.close()
           
                try:
                    q_obj.join_thread()
                except RuntimeError:
                     # join_thread might raise RuntimeError if the thread wasn't started or already joined
                     pass
                print(f"[主进程] 队列已关闭: {q_obj}")
            except Exception as e_q:
                print(f"[主进程] 关闭队列 {q_obj} 时出错: {e_q}")

        cv2.destroyAllWindows()
        print("[主进程] 应用程序已关闭。")



if __name__ == "__main__":
    multiprocessing.freeze_support()
    main_process()